#include "command.h"
#include "os/os.h"
#include "os/queue.h"
#include "utils/utils.h"
#include "utils/logger.h"
#include "protocols/protocol.h"
#include "controller/motor.h"

static co_queue_t _cmd_queue;
static command_handler_t command_handler[CMD_Number];

static u32 command_package_task(void *args);
static void process_command(package_t *command);
static void command_handler_init(void);

#define motctrl() controller(motor()) 

void command_package_init(void) {
    command_handler_init();
	_cmd_queue = queue_create(16, sizeof(package_t));
	os_task_create(command_package_task, NULL);
}

bool process_command_package(package_t *command) {
	if (!queue_put(_cmd_queue, command)) {
		if (command->data) {
			os_free(command->data);
		}
		return false;
	}
	return true;
}

static u32 command_package_task(void *args) {
	package_t command;
	if (queue_get(_cmd_queue, &command)) {
		process_command(&command);
		if (command.data) {
			os_free(command.data);
		}
	}
	return 0;
}


static void process_command(package_t *pack) {
    u8 response[64];
    int len = 0;

    command_handler_t handler = NULL;
    if (pack->command < CMD_Number) {
        handler = command_handler[pack->command];
    }
    if ( handler != NULL) {
        len = handler(pack, response);
    }else {
        len = 1;
        response[0] = 0xFF; //command not support
    }
    if (len) {
	    protocol_send_response(pack->command, response, len);
    }
}


static int command_startstop(package_t *pack, u8 *response) {
    u8 cmd = decode_u8(pack->data);
    bool success = false;
    response[0] = 0;
    if (cmd) {
        success = motor_start(motor(0), CTRL_MODE_TORQUE);
    }else {
        success = motor_stop(motor(0));
    }
    if (!success) {
        response[0] = 1;
    }
    return 1;
}

static int command_set_ctrlmode(package_t *pack, u8 *response) {
    u8 mode = decode_u8(pack->data);
    bool success = false;
    response[0] = 0;
	success = contrl_request_mode(controller(motor(0)), mode);
    if (!success) {
        response[0] = 1;
    }
    return 1;
}

static int command_set_vdq(package_t *pack, u8 *response) {
    u8 vd = decode_u8(pack->data);
	u8 vq = decode_u8(pack->data + 1);
    bool success = false;
    response[0] = 0;
	motor_set_target_vdq(motor(0), (float)vd, (float)vq);
	sys_debug("set vdq %d, %d\n", vd, vq);
    return 1;
}

static int command_set_torque(package_t *pack, u8 *response) {
    s16 torque = decode_s16(pack->data);
    bool success = false;
    response[0] = 0;
	motor_set_target_torque(motor(0), (float)torque);
	sys_debug("set torque %d\n", torque);
    return 1;
}

static int command_set_currdq(package_t *pack, u8 *response) {
    s16 id = decode_s16(pack->data);
	s16 iq = decode_s16(pack->data + 2);
    bool success = false;
    response[0] = 0;
	motor_set_target_currdq(motor(0), (float)id, (float)iq);
	sys_debug("set curr dq %d, %d\n", id, iq);
    return 1;
}

static int command_set_plots(package_t *pack, u8 *response) {
    int num = pack->len;
    if (num > PLOT_MAX_IDS) {
        num = PLOT_MAX_IDS;
    }
    for (int i = 0; i < num; i++) {
        motor(0)->plot_ids[i] = pack->data[i];
    }
    response[0] = 0;
    return 1;
}

static int command_force_run_open(package_t *pack, u8 *response) {
    float vd = (float)decode_s16(pack->data) / 10.0f;
	motor_t *mot = motor(0);
    response[0] = 0;
	float vel = 0;
	if (pack->len > 2) {
		vel = (float)decode_float(pack->data + 2);
	}
	sys_debug("Force vd=%f, %f\n", vd, (float)vel);
	motor_force_foc_angle(mot, vd, vel);

    return 1;
}


static void register_command_handler(u8 command, command_handler_t handler) {
    if (command < CMD_Number) {
        command_handler[command] = handler;
    }
}


static void command_handler_init(void) {
    register_command_handler(CMD_Start_Motor, command_startstop);
	register_command_handler(CMD_Set_CtrlMode, command_set_ctrlmode);
	register_command_handler(CMD_Set_Target_Vdq, command_set_vdq);
	register_command_handler(CMD_Set_Target_Torque, command_set_torque);
	register_command_handler(CMD_Set_Target_Currdq, command_set_currdq);
	register_command_handler(CMD_Set_Plot_Type, command_set_plots);
	register_command_handler(CMD_Force_Open_Run, command_force_run_open);
}
